#include "include.h"

int8 ServoDeviation[8] ={0,0,0,18,18,0,90,0}; //下面ServoPwmDuty与1500的偏差值
uint16 ServoPwmDuty[8] = {1500,1500,1500,1482,2482,1500,1410,1500};	//目前PWM脉冲宽度
uint16 ServoPwmDutySet[8] = {1500,1500,1500,1500,1500,1500,1500,1500};	//目标PWM脉冲宽度
float  ServoPwmDutyInc[8];		//为了速度控制，当PWM脉宽发生变化时，每2.5ms或20ms递增的PWM脉宽

bool ServoPwmDutyHaveChange = FALSE;	//脉宽有变化标志位

uint16 ServoTime = 2000;			//舵机从当前角度运动到指定角度的时间，也就是控制速度

 
//初始化定时器3时基配置为1ms
void InitTimer3(void)
{
	NVIC_InitTypeDef NVIC_InitStructure;
	
	RCC->APB1ENR|=1<<1;//TIM3时钟使能
 	TIM3->ARR=10000 - 1;  //设定计数器自动重装值//刚好1ms    
	TIM3->PSC=72 - 1;  //预分频器72,得到1Mhz的计数时钟
	//这两个东东要同时设置才可以使用中断
	TIM3->DIER|=1<<0;   //允许更新中断				
//	TIM3->DIER|=1<<6;   //允许触发中断	   
	TIM3->CR1|=0x01;    //使能定时器3
	
	NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;  //TIM3中断
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;  //先占优先级3级
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;	//从优先级3级
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能
	NVIC_Init(&NVIC_InitStructure);  //根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器
}

//初始化定时器3输出PWM的IO口
void InitPWM(void)
{
	GPIO_InitTypeDef  GPIO_InitStructure;
	
	InitTimer3();
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_8;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;		 //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);

	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10| GPIO_Pin_11 | GPIO_Pin_12;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;		 //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOC, &GPIO_InitStructure);
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE);
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;		 //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOD, &GPIO_InitStructure);
}

//将PWM脉宽转化成自动装载寄存器的值
//即修改中断时间
void Timer3ARRValue(uint16 pwm)	
{
	TIM3->ARR = pwm + 1;
}


//定时器3中断服务程序	 
//只有舵机1-6所以注释掉舵机0和7
//将舵机周期20ms均分为8个小周期2500us
//每个小周期控制一个舵机信号线的高低电平
//这样就可以实现在20ms里控制8个舵机
//而宏观看上去舵机同时控制
void TIM3_IRQHandler(void)
{ 		
	static uint16 i = 1;
	
	if(TIM3->SR&0X0001)//溢出中断
	{
		switch(i)
		{
			case 1:
// 				SERVO0 = 1;	//PWM控制脚高电平
				//给定时器0赋值，计数Pwm0Duty个脉冲后产生中断，下次中断会进入下一个case语句
				Timer3ARRValue(ServoPwmDuty[0]);
				break;
			case 2:
// 				SERVO0 = 0;	//PWM控制脚低电平
				//此计数器赋值产生的中断表示下一个单元要进行任务的开始
				Timer3ARRValue(2500-ServoPwmDuty[0]);	
				break;
			case 3:
				SERVO1 = 1;	
				Timer3ARRValue(ServoPwmDuty[1]);
				break;
			case 4:
				SERVO1 = 0;	//PWM控制脚低电平
				Timer3ARRValue(2500-ServoPwmDuty[1]);	
				break;
			case 5:
				SERVO2 = 1;	
				Timer3ARRValue(ServoPwmDuty[2]);
				break;
			case 6:
				SERVO2 = 0;	//PWM控制脚低电平
				Timer3ARRValue(2500-ServoPwmDuty[2]);	
				break;	
			case 7:
				SERVO3 = 1;	
				Timer3ARRValue(ServoPwmDuty[3]);
				break;
			case 8:
				SERVO3 = 0;	//PWM控制脚低电平
				Timer3ARRValue(2500-ServoPwmDuty[3]);	
				break;	
			case 9:
				SERVO4 = 1;	
				Timer3ARRValue(ServoPwmDuty[4]);
				break;
			case 10:
				SERVO4 = 0;	//PWM控制脚低电平
				Timer3ARRValue(2500-ServoPwmDuty[4]);	
				break;	
			case 11:
				SERVO5 = 1;	
				Timer3ARRValue(ServoPwmDuty[5]);
				break;
			case 12:
				SERVO5 = 0;	//PWM控制脚低电平
				Timer3ARRValue(2500-ServoPwmDuty[5]);	
				break;
			case 13:
				SERVO6 = 1;	
				Timer3ARRValue(ServoPwmDuty[6]);
				break;
			case 14:
				SERVO6 = 0;	//PWM控制脚低电平
				Timer3ARRValue(2500-ServoPwmDuty[6]);	
				break;
			case 15:
// 				SERVO7 = 1;	
				Timer3ARRValue(ServoPwmDuty[7]);
				break;
			case 16:
// 				SERVO7 = 0;	//PWM控制脚低电平
				Timer3ARRValue(2500-ServoPwmDuty[7]);
				i = 0;	
				break;				 
		}
		i++;
	}				   
	TIM3->SR&=~(1<<0);//清除中断标志位 	    
}

//设置舵机转动的目标位置和转动时间的函数。
//检查舵机的号数是否在0-7之间
//位置是否在500道2500之间
//如果不符合就 直接退出函数
//符合的话检查时间，保证时间在20道30000之间
//然后将位置写入数组，时间写入变量,设置舵机已经被设置的标志
void ServoSetPluseAndTime(uint8 id,uint16 p,uint16 time)
{
	if(	p - ServoPwmDuty[id] != ServoDeviation[id])
		if(id >= 0 && id <= 7 && p >= 500 && p <= 2500)
		{
			if(time < 20)
				time = 20;
			if(time > 30000)
				time = 30000;
			ServoPwmDutySet[id] = p - ServoDeviation[id];
			ServoTime = time;
			ServoPwmDutyHaveChange = TRUE;
		}
	
}

void ServoPwmDutyCompare(void)//脉宽变化比较及速度控制
{
	uint8 i;
	
	static uint16 ServoPwmDutyIncTimes;	//需要递增的次数
	static bool ServoRunning = FALSE;	//舵机正在以指定速度运动到指定的脉宽对应的位置
	if(ServoPwmDutyHaveChange)//停止运动并且脉宽发生变化时才进行计算      ServoRunning == FALSE && 
	{
		ServoPwmDutyHaveChange = FALSE;
		ServoPwmDutyIncTimes = ServoTime/20;	//当每20ms调用一次ServoPwmDutyCompare()函数时用此句 2000/20=100
		for(i=0;i<8;i++)
		{
			//if(ServoPwmDuty[i] != ServoPwmDutySet[i])
			{
				if(ServoPwmDutySet[i] > ServoPwmDuty[i])
				{
					ServoPwmDutyInc[i] = ServoPwmDutySet[i] - ServoPwmDuty[i];
					ServoPwmDutyInc[i] = -ServoPwmDutyInc[i];
				}
				else
				{
					ServoPwmDutyInc[i] = ServoPwmDuty[i] - ServoPwmDutySet[i];
					
				}
				ServoPwmDutyInc[i] /= ServoPwmDutyIncTimes;//每次递增的脉宽 将目标和目前脉宽差细分2000/20=100份
			}
		}
		ServoRunning = TRUE;	//舵机开始动作
	}
	if(ServoRunning)    //舵机开始运动
	{
		ServoPwmDutyIncTimes--;
		for(i=0;i<8;i++)
		{
			if(ServoPwmDutyIncTimes == 0)
			{		//最后一次递增就直接将设定值赋给当前值

				ServoPwmDuty[i] = ServoPwmDutySet[i];

				ServoRunning = FALSE;	//到达设定位置，舵机停止运动
			}
			else
			{

				ServoPwmDuty[i] = ServoPwmDutySet[i] + 
				(signed short int)(ServoPwmDutyInc[i] * ServoPwmDutyIncTimes);//不理解，差的1/100乘上99是为什么

			}
		}
		
	}
}
